#!/usr/bin/env python
#coding:utf-8
import rospy

from px4flow.msg import Flow
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from std_msgs.msg import Header
from serial import Serial
from nav_msgs.msg import Odometry
from struct import *
import time
import numpy as np


ser=Serial("/dev/ttyS0",19200,timeout=0.5)

# SCALE大小，飞机解析时设置同样数值
SCALE_F = 10000.0
current_pos = np.array([0.0,0.0,0.0])
current_angle = np.array([0.0,0.0,0.0])

def send_t265_data(current_pos,current_angle,receive_flag):
    _roll = int(SCALE_F*current_angle[0])
    _pitch = int(SCALE_F*current_angle[1])
    _yaw = int(SCALE_F*current_angle[2])
    _x = int(SCALE_F*current_pos[0])
    _y = int(SCALE_F*current_pos[1])
    _z = int(SCALE_F*current_pos[2])
    # 21 send ;20 ignore 0XFE,0X22,0x14(发送)，0x15(接收)
    data = pack('>2b7ib',-2,34,_x,_y,_z,_roll,_pitch,_yaw,(_x+_y+_z+_roll+_pitch+_yaw),receive_flag)
    ser.write(data)

def callback(odom):
    global hz
    global current_pos
    global current_angle
    # print(odom)
    # origin Quaternion data
    x1 = odom.pose.pose.orientation.x
    y1 = odom.pose.pose.orientation.y
    z1 = odom.pose.pose.orientation.z
    w1 = odom.pose.pose.orientation.w
    (roll, pitch, yaw) = euler_from_quaternion ([x1,y1,z1,w1])
    current_pos[0] = odom.pose.pose.position.x
    current_pos[1] = odom.pose.pose.position.y
    current_pos[2] = odom.pose.pose.position.z
    current_angle[0] = roll
    current_angle[1] = pitch
    current_angle[2] = yaw

def start():
    # starts the node
    rospy.init_node('Realsense2Serial')
    # some global variable
    if ser.isOpen():
        rospy.loginfo("open success")
    else:
        rospy.loginfo("open failed")   
    # some global variable    
    global current_pos
    global current_angle
    # subscribed to realsense inputs on topic "odom"
    # rospy.Subscriber("/camera/odom/sample", Odometry, callback)
    rospy.Subscriber("/fused_odom", Odometry, callback)
    pub = rospy.Publisher('/DistanceAcc', Flow, queue_size=10)
    loop_rate = rospy.Rate(40)
    receive_flag = 20
    msg = Flow()
    rbytes = bytearray()
    while rospy.is_shutdown is not True:
        send_t265_data(current_pos,current_angle,receive_flag)
        # data = pack('>2b7ib',-2,34,_x,_y,_z,_roll,_pitch,_yaw,(_x+_y+_z+_roll+_pitch+_yaw),(20+receive_flag))
        # rospy.loginfo("send success "+str(receive_flag))
        # ser.write(data)
        if receive_flag == 21:
            # read_data = []
            rbytes[0:21] = b''
            data_received = False
            for i in range(10):
                data = ser.read()
                if len(data) == 1:
                    data = unpack('>b',data)
                    if data[0] is -2:
                        data = ser.read()
                        # if(data is '"'): # Only check 0xFE. 0x22 is ignored.
                        data_received = True
                        break

            if data_received:
                for i in range(0,21):
                    data = ser.read()
                    rbytes = rbytes+data
                    #print(repr(rbytes))
                    # print(str(read_data[i])+str(i))
                #  byte3-0
                # print(len(rbytes))
                if len(rbytes) == 21:
                    distance,accx,accy,_yaw,sum,rend = unpack('>5ib',rbytes)
                    check = (sum == distance+accx+accy+_yaw)
                    if check is True:
                        distance = distance/SCALE_F
                        accx = accx/SCALE_F
                        accy = accy/SCALE_F
                        _yaw = _yaw/SCALE_F

                        rospy.loginfo("distance"+str(distance)+"accx"+str(accx)+"accy"+str(accy)+"yaw"+str(_yaw)+"   "+str(sum)+'  '+str(check))
                        header = Header(stamp=rospy.Time.now())
                        header.frame_id = 'Distance in hight acc in pos'
                        msg.header = header
                        msg.hight = distance
                        msg.pos[0] = accx
                        msg.pos[1] = accy
                        msg.pos[2] = _yaw
                        pub.publish(msg)
                    else:
                        rospy.logwarn("Check failed!!!!!!")
                        #return
                # raise NameError
        receive_flag = receive_flag+1
        if receive_flag == 22:
            receive_flag = 20
        loop_rate.sleep()
    rospy.spin()
if __name__ == '__main__':
    start()